home *** CD-ROM | disk | FTP | other *** search
/ BCI NET 2 / BCI NET 2.iso / archives / programming / source / graphicgems3.lha / GemsIII / sqfinal.c < prev    next >
Encoding:
C/C++ Source or Header  |  1995-02-06  |  9.4 KB  |  324 lines
  1. /*
  2.  * This code calculates the volume, mass, and inertia tensors of 
  3.  * superquadric ellipsoids and toroids. The code includes methods
  4.  * to numerically compute gamma functions and beta functions
  5.  */
  6.  
  7. #include <stdio.h>
  8. #include <math.h>
  9.  
  10. /*
  11.  * The following function, abgam() is based on a continued fraction numerical
  12.  * method found in Abremowitz and Stegun, Handbook of Mathematical Functions
  13.  * 
  14.  */
  15. double  abgam (x)
  16. double  x;
  17. {
  18.   double  gam[10],
  19.           result,
  20.           temp;
  21.  
  22.   gam[0] = 1./ 12.;
  23.   gam[1] = 1./ 30.;
  24.   gam[2] = 53./ 210.;
  25.   gam[3] = 195./ 371.;
  26.   gam[4] = 22999./ 22737.;
  27.   gam[5] = 29944523./ 19733142.;
  28.   gam[6] = 109535241009./ 48264275462.;
  29.   temp = 0.5*log (2*M_PI) - x + (x - 0.5)*log (x)
  30.     + gam[0]/(x + gam[1]/(x + gam[2]/(x + gam[3]/(x + gam[4] /
  31.       (x + gam[5]/(x + gam[6]/x))))));
  32.  
  33.   return temp;
  34. }
  35.  
  36.  
  37.  
  38. /* 
  39.  * A method to compute the gamma() function.
  40.  *
  41.  */  
  42. double  gamma (x)
  43. double  x;
  44. {
  45.   double  result,
  46.           abgam ();
  47.   result = exp (abgam (x + 5))/(x*(x + 1)*(x + 2)*(x + 3)*(x + 4));
  48.   return result;
  49. }
  50.  
  51. /* 
  52.  * A method to compute the beta() function.
  53.  */  
  54. double  beta (m, n)
  55. double  m,
  56.         n;
  57. {
  58.   double  gamma ();
  59.   return (gamma (m)*gamma (n)/gamma (m + n));
  60. }
  61.  
  62.  
  63. /* 
  64.  * A method to compute the volume of a superquadric ellipsoid
  65.  * with axis lengths a1, a2, a3, north-south exponent n
  66.  * and east-west exponent e
  67.  */  
  68. double  sqellipvol (a1, a2, a3, n, e)
  69. double  a1,                               /* x radius */ 
  70.         a2,                               /* y radius */
  71.         a3,                               /* z radius */
  72.         n,                                /* north-south param */
  73.         e;                                /* east-west param */
  74. {
  75.   double  beta ();
  76.   return ((2./ 3.)*a1*a2*a3*e*n*beta (e/2., e/2.)*beta (n, n/2.));
  77. }
  78.  
  79.  
  80. /* 
  81.  * A method to compute the volume of a superquadric toroid
  82.  * with axis lengths a1, a2, a3, north-south exponent n
  83.  * east-west exponent e, and hole parameter alpha
  84.  */  
  85. double  sqtoroidvol (a1, a2, a3, n, e, alpha)
  86. double  a1,                               /* x radius */ 
  87.         a2,                               /* y radius */
  88.         a3,                               /* z radius */
  89.         n,                                /* north-south param */
  90.         e,                                /* east-west param */
  91.         alpha;                            /* torus hole-size parameter */
  92. {
  93.   return (2.*a1*a2*a3*alpha*e*n*beta (e/2., e/2.)*beta (n/2., n/2.));
  94.  
  95. }
  96.  
  97.  
  98.  
  99. /* 
  100.  * A procedure to print the inertia tensor of a canonical superquadric 
  101.  * ellipsoid in its body coordinate system. 
  102.  */  
  103. void sq_ellipsoid_tensor (rho, a1, a2, a3, e, n)
  104. double  a1,
  105.         a2,
  106.         a3,
  107.         e,
  108.         n,
  109.         rho;                     /* density of material */
  110. {
  111.   double  iellip[3][3],
  112.           i1E,
  113.           i2E,
  114.           i3E,
  115.           iworld[3][3],
  116.           R[3][3];
  117.   i1E = (2./ 5.)* a1*a1*a1*a2*a3*e*n*beta(3.* e/2., e/2.)*beta(2.* n, n/2.);
  118.   i2E = (2./ 5.)* a1*a2*a2*a2*a3*e*n*beta(e/2., 3.* e/2.)*beta(2.* n, n/2.);
  119.   i3E = (2./ 5.)* a1*a2*a3*a3*a3*e*n*beta(e/2., e/2.)*beta(n, 3.* n/2.);
  120.  
  121.   iellip[0][0] = 0;
  122.   iellip[1][0] = 0;
  123.   iellip[2][0] = 0;
  124.   iellip[0][1] = 0;
  125.   iellip[1][1] = 0;
  126.   iellip[2][1] = 0;
  127.   iellip[0][2] = 0;
  128.   iellip[1][2] = 0;
  129.   iellip[2][2] = 0;
  130.   
  131.   iellip[0][0] = i2E + i3E;
  132.   iellip[1][1] = i1E + i3E;
  133.   iellip[2][2] = i1E + i2E;
  134.   
  135.   printf ("ellipsoid inertia tensor in body coordinates\n");
  136.  
  137.   printf ("  iellip1 = %f %f %f \n", iellip[0][0], iellip[1][0], iellip[2][0]);
  138.   printf ("  iellip2 = %f %f %f \n", iellip[0][1], iellip[1][1], iellip[2][1]);
  139.   printf ("  iellip3 = %f %f %f \n", iellip[0][2], iellip[1][2], iellip[2][2]);
  140.  
  141. }
  142.  
  143.  
  144. /* 
  145.  * A procedure to print the inertia tensor of a canonical superquadric 
  146.  * toroid in its body coordinate system. 
  147.  */  
  148. void sq_toroid_tensor (rho, a1, a2, a3, e, n, alpha)
  149. double  rho,
  150.         a1,
  151.         a2,
  152.         a3,
  153.         e,
  154.         n,
  155.         alpha;
  156. {
  157.   double  itor[3][3],
  158.           i1T,
  159.           i2T,
  160.           i3T;
  161.   i1T = a1*a1*a1*a2*a3*alpha*e*n*beta(3*e/2,e/2)*(2*alpha*alpha*beta(n/2,n/2)+
  162.       3*beta(3*n/2,n/2));
  163.   i2T = a1*a2*a2*a2*a3*alpha*e*n*beta(e/2,3*e/2)*(2*alpha*alpha*beta(n/2,n/2)+
  164.       3*beta(3*n/2,n/2));
  165.   i3T = a1*a2*a3*a3*a3*alpha*e*n*beta(e/2,e/2)*beta(n/2,3*n/2);
  166.  
  167.     itor[0][0] = 0;
  168.     itor[1][0] = 0;
  169.     itor[2][0] = 0;
  170.     itor[0][1] = 0;
  171.     itor[1][1] = 0;
  172.     itor[2][1] = 0;
  173.     itor[0][2] = 0;
  174.     itor[1][2] = 0;
  175.     itor[2][2] = 0;
  176.     itor[0][0] = i2T + i3T;
  177.     itor[1][1] = i1T + i3T;
  178.     itor[2][2] = i1T + i2T;
  179.     printf ("toroid inertia tensor in body coordinates\n");
  180.     printf ("  itor1  = %f %f %f \n", itor[0][0], itor[1][0], itor[2][0]);
  181.     printf ("  itor2  = %f %f %f \n", itor[0][1], itor[1][1], itor[2][1]);
  182.     printf ("  itor3  = %f %f %f \n", itor[0][2], itor[1][2], itor[2][2]);
  183. }
  184.  
  185. /* 
  186.  * A procedure to print the inertia tensor components in world coordinates,
  187.  * given an inertia tensor in body coordinates, and the 3x3 rotation matrix
  188.  * which rotates body vectors into world coordinates.  
  189.  */  
  190. void iworld (Ibody, R)
  191. double  Ibody[3][3],
  192.          R[3][3];
  193. {
  194.   double  Iworld[3][3];
  195.  Iworld[0][0] =
  196.     (R[0][0]*Ibody[0][0]+R[0][1]*Ibody[1][0]+R[0][2]*Ibody[2][0])*R[0][0] +
  197.     (R[0][0]*Ibody[0][1]+R[0][1]*Ibody[1][1]+R[0][2]*Ibody[2][1])*R[0][1] +
  198.     (R[0][0]*Ibody[0][2]+R[0][1]*Ibody[1][2]+R[0][2]*Ibody[2][2])*R[0][2];
  199.   
  200.   Iworld[1][0] =
  201.      (R[1][0]*Ibody[0][0]+R[1][1]*Ibody[1][0]+R[1][2]*Ibody[2][0])*R[0][0]+
  202.      (R[1][0]*Ibody[0][1]+R[1][1]*Ibody[1][1]+R[1][2]*Ibody[2][1])*R[0][1]+
  203.      (R[1][0]*Ibody[0][2]+R[1][1]*Ibody[1][2]+R[1][2]*Ibody[2][2])*R[0][2];
  204.   
  205.   Iworld[2][0] =
  206.      (R[2][0]*Ibody[0][0]+R[2][1]*Ibody[1][0]+R[2][2]*Ibody[2][0])*R[0][0]+
  207.      (R[2][0]*Ibody[0][1]+R[2][1]*Ibody[1][1]+R[2][2]*Ibody[2][1])*R[0][1]+
  208.      (R[2][0]*Ibody[0][2]+R[2][1]*Ibody[1][2]+R[2][2]*Ibody[2][2])*R[0][2];
  209.  
  210.   Iworld[0][1] =
  211.      (R[0][0]*Ibody[0][0]+R[0][1]*Ibody[1][0]+R[0][2]*Ibody[2][0])*R[1][0]+
  212.      (R[0][0]*Ibody[0][1]+R[0][1]*Ibody[1][1]+R[0][2]*Ibody[2][1])*R[1][1]+
  213.      (R[0][0]*Ibody[0][2]+R[0][1]*Ibody[1][2]+R[0][2]*Ibody[2][2])*R[1][2];
  214.   
  215.   Iworld[1][1] =
  216.      (R[1][0]*Ibody[0][0]+R[1][1]*Ibody[1][0]+R[1][2]*Ibody[2][0])*R[1][0]+
  217.      (R[1][0]*Ibody[0][1]+R[1][1]*Ibody[1][1]+R[1][2]*Ibody[2][1])*R[1][1]+
  218.      (R[1][0]*Ibody[0][2]+R[1][1]*Ibody[1][2]+R[1][2]*Ibody[2][2])*R[1][2];
  219.   
  220.   Iworld[2][1] =
  221.      (R[2][0]*Ibody[0][0]+R[2][1]*Ibody[1][0]+R[2][2]*Ibody[2][0])*R[1][0]+
  222.      (R[2][0]*Ibody[0][1]+R[2][1]*Ibody[1][1]+R[2][2]*Ibody[2][1])*R[1][1]+
  223.      (R[2][0]*Ibody[0][2]+R[2][1]*Ibody[1][2]+R[2][2]*Ibody[2][2])*R[1][2];
  224.    
  225.   Iworld[0][2] =
  226.      (R[0][0]*Ibody[0][0]+R[0][1]*Ibody[1][0]+R[0][2]*Ibody[2][0])*R[2][0]+
  227.      (R[0][0]*Ibody[0][1]+R[0][1]*Ibody[1][1]+R[0][2]*Ibody[2][1])*R[2][1]+
  228.      (R[0][0]*Ibody[0][2]+R[0][1]*Ibody[1][2]+R[0][2]*Ibody[2][2])*R[2][2];
  229.   
  230.   Iworld[1][2] =
  231.      (R[1][0]*Ibody[0][0]+R[1][1]*Ibody[1][0]+R[1][2]*Ibody[2][0])*R[2][0]+
  232.      (R[1][0]*Ibody[0][1]+R[1][1]*Ibody[1][1]+R[1][2]*Ibody[2][1])*R[2][1]+
  233.      (R[1][0]*Ibody[0][2]+R[1][1]*Ibody[1][2]+R[1][2]*Ibody[2][2])*R[2][2];
  234.   
  235.   Iworld[2][2] =
  236.      (R[2][0]*Ibody[0][0]+R[2][1]*Ibody[1][0]+R[2][2]*Ibody[2][0])*R[2][0]+
  237.      (R[2][0]*Ibody[0][1]+R[2][1]*Ibody[1][1]+R[2][2]*Ibody[2][1])*R[2][1]+
  238.      (R[2][0]*Ibody[0][2]+R[2][1]*Ibody[1][2]+R[2][2]*Ibody[2][2])*R[2][2];
  239.   
  240.   printf ("toroid inertia tensor in body coordinates\n");
  241.   printf (" Iworld1 = %f %f %f \n", Iworld[0][0], Iworld[1][0], Iworld[2][0]);
  242.   printf (" Iworld2 = %f %f %f \n", Iworld[0][1], Iworld[1][1], Iworld[2][1]);
  243.   printf (" Iworld3 = %f %f %f \n", Iworld[0][2], Iworld[1][2], Iworld[2][2]);
  244. }
  245.  
  246. /*
  247.  * sgn(x) returns -1.0 or 1.0 for speed. 
  248.  * sgn(x) can return 0.0 on zero if you wish, depending on
  249.  * your convention 
  250.  */ 
  251. double sgn(x)
  252. double x;
  253. {
  254.   if (x <= 0.0) return (-1.0);
  255.   else return(1.0);
  256. } 
  257.   
  258.  
  259. /*
  260.  * computes position on the surface of a superquadric ellipsoid
  261.  * v goes from -Pi/2 to Pi/2; u  goes from -Pi to Pi. 
  262.  *
  263.  */ 
  264. void sqellipsoidposn(a1,a2,a3,n,e,alpha,u,v)
  265. double a1,a2,a3,n,e,alpha,u,v;
  266. {
  267.   double cu, su, cv, sv, x,y,z;
  268.   cu = cos(u);
  269.   su = sin(u);
  270.   cv = cos(v);
  271.   sv = cos(v);
  272.  
  273.   x = a1*(alpha + pow(cv,n))*pow(cu,e)*sgn(cu)*sgn(cv);
  274.   y = a2*(alpha + pow(cv,n))*pow(su,e)*sgn(su)*sgn(cv);
  275.   z = a3*pow(sv,n)*sgn(sv);
  276. }  
  277.  
  278.  
  279. /*
  280.  * computes position on the surface of a superquadric toroid
  281.  * u and v go from -Pi to Pi
  282.  */
  283.  
  284. void sqtoroidposn(a1,a2,a3,n,e,u,v)
  285. double a1,a2,a3,n,e,u,v;
  286. {
  287.   double cu, su, cv, sv, x,y,z;
  288.   cu = cos(u);
  289.   su = sin(u);
  290.   cv = cos(v);
  291.   sv = cos(v);
  292.  
  293.   x = a1*pow(cv,n)*pow(cu,e)*sgn(cu)*sgn(cv);
  294.   y = a2*pow(cv,n)*pow(su,e)*sgn(su)*sgn(cv);
  295.   z = a3*pow(sv,n)*sgn(sv);
  296.  
  297. }  
  298.  
  299. /*  
  300.  * A procedure to test some of the above code
  301.  * 
  302.  */ 
  303. main () {
  304.  double x;
  305.   printf (" gamma(1)= 1.0 = %12.10lf\n", gamma (1.0));
  306.   printf (" gamma(1/2)^2= Pi =%12.10lf\n", gamma (0.5)*gamma (0.5));
  307.   printf (" gamma(2)= 1.0 = %12.10lf\n", gamma (2.0));
  308.   printf (" gamma(3)= 2.0 = %12.10lf\n", gamma (3.0));
  309.   printf (" gamma(4)= 6.0 = %12.10lf\n", gamma (4.0));
  310.   printf("\n");
  311.   printf ("beta(1,1)= 1.0 = %12.10lf\n", beta (1.0, 1.0));
  312.   printf ("beta(1,1/2)= 2.0 = %12.10lf\n", beta (1.0, 0.5));
  313.   printf ("beta(1/2,1/2)= Pi = %12.10lf\n", beta (0.5, 0.5));
  314.   printf("\n");
  315.   printf ("sq ellipsoid volume/pi= 4/3 = %12.10lf\n",
  316.                       sqellipvol(1., 1., 1., 1., 1.)/M_PI);
  317.   printf ("sq toroid volume/pi^2 = 2.0 = %12.10lf\n",
  318.            sqtoroidvol (1., 1., 1., 1., 1., 1.)/M_PI/M_PI);
  319.   printf("\n");
  320.   sq_ellipsoid_tensor (1., 1., 1., 1., 1., 1., 1.);
  321.   sq_toroid_tensor (1., 1., 1., 1., 1., 1., 1.);
  322. }
  323.  
  324.